using System;
using BepuUtilities;
using BepuPhysics;
using DemoContentLoader;
using DemoRenderer;
using BepuPhysics.Collidables;
using DemoUtilities;
using DemoRenderer.UI;
using BepuPhysics.CollisionDetection;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using System.Runtime.CompilerServices;
using BepuPhysics.Constraints;
using System.Diagnostics;
using System.Threading;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace Demos.Demos
{
    // BepuPhysical v2没有任何"事件"的概念。它具有报告触点歧管当前状态的回调。
    // 可以围绕这些回调构建事件。这个演示展示了实现这一点的一种方法。

    // 关于此事件处理程序方法,有几点值得注意：
    // 1)每个联系事件都通过保存事件处理程序逻辑传递。它不支持将独特的逻辑连接到单独的可碰撞对象。
    // 可以通过将其重写为更惯用一点、将处理程序包装在另一个使用委托的系统中或等待C#支持非托管委托来解决此问题：
    // https://github.com/dotnet/csharplang/blob/master/proposals/static-delegates.md
    // 2)这只实现"添加联系人时"事件。添加OnContactRemoved或OnTouching事件并不是很困难。
    // 3)所有事件处理程序都从模拟执行的多线程上下文中执行,因此您必须注意事件处理程序的操作。
    // 可以在此基础上构建"延迟"事件模型。
    // 4)此模型不公开联系人数据以供直接修改;它严格作为通知操作。
    // 5)这做了相当多的工作,并放慢了狭窄阶段的速度。
    // 6)这无法深入了解与这些联系人相关联的约束。例如,如果要繁殖粒子以响应重碰撞或播放声音
    // 由于体积取决于撞击力,因此需要从解算器中提取冲量数据。此外,这些类型的用例通常只适用于附近的对象,
    // 因此,基于监听器的模型并不理想。相反,查询附近的活动对象并检查它们的接触约束会更加直接。
    // 7)可以推测性地创建联系人。现有的触点不能保证非负的穿透深度！我们不会试图在演示中隐藏这一事实。
    // 您可以将其修改为仅将非负深度接触视为出于事件目的而存在的,但它会变得更加复杂。
    // 8)还有其他拉取联系人数据的方式。例如,查看接触线提取器在DemoRenender中是如何工作的。
    // 它直接从求解器数据中提取数据,并可以扩展到提取其他信息,如脉冲。

    public interface IContactEventHandler
    {
        void OnContactAdded<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold,
            in Vector3 contactOffset, in Vector3 contactNormal, float depth, int featureId, int contactIndex, int workerIndex) where TManifold : struct, IContactManifold<TManifold>;
    }

    public unsafe class ContactEvents<TEventHandler> : IDisposable where TEventHandler : IContactEventHandler
    {
        struct PreviousCollisionData
        {
            public CollidableReference Collidable;
            public bool Fresh;
            public int ContactCount;
            // FeatureID是编码所涉及形状上的哪些特征对联系人有贡献的标识符。我们最多存储4个功能ID,每个潜在联系人一个。
            // "Feature"指的是面、顶点或边之类的东西。对于什么是特征没有单一的解释-映射是在每个碰撞对级别上定义的。
            // 在此演示中,我们只检查当前帧中的给定联系人是否映射到前一帧中的联系人。
            // 我们可以使用它仅在报告具有无法识别ID的新联系人时发出"已添加联系人"事件。
            public int FeatureId0;
            public int FeatureId1;
            public int FeatureId2;
            public int FeatureId3;
        }

        Bodies bodies;
        public TEventHandler EventHandler;
        BufferPool pool;
        IThreadDispatcher threadDispatcher;


        QuickDictionary<CollidableReference, QuickList<PreviousCollisionData>, CollidableReferenceComparer> listeners;

        // 因为窄阶段在多个线程上工作,所以我们不能在执行期间修改冲突数据。
        // 挂起的更改存储在每个工作进程集合中,并在以后刷新。
        struct PendingNewEntry
        {
            public int ListenerIndex;
            public PreviousCollisionData Collision;
        }

        QuickList<PendingNewEntry>[] pendingWorkerAdds;

        public ContactEvents(TEventHandler eventHandler, BufferPool pool, IThreadDispatcher threadDispatcher, int initialListenerCapacity = 32)
        {
            this.EventHandler = eventHandler;
            this.pool = pool;
            this.threadDispatcher = threadDispatcher;
            pendingWorkerAdds = new QuickList<PendingNewEntry>[threadDispatcher == null ? 1 : threadDispatcher.ThreadCount];
            listeners = new QuickDictionary<CollidableReference, QuickList<PreviousCollisionData>, CollidableReferenceComparer>(initialListenerCapacity, pool);
        }

        public void Initialize(Bodies bodies)
        {
            this.bodies = bodies;
        }

        /// <summary>
        /// 开始侦听与给定的可碰撞对象相关的事件。
        /// </summary>
        /// <param name="Collisible">可冲突地监视事件。</param>
        public void RegisterListener(CollidableReference collidable)
        {
            listeners.Add(collidable, default, pool);
        }

        /// <summary>
        /// 停止侦听与给定的可碰撞对象相关的事件。
        /// </summary>
        /// <param name="Collisible">停止侦听的冲突。</param>
        public void UnregisterListener(CollidableReference collidable)
        {
            var exists = listeners.GetTableIndices(ref collidable, out var tableIndex, out var elementIndex);
            Debug.Assert(exists, "Should only try to unregister listeners that actually exist.");
            listeners.Values[elementIndex].Dispose(pool);
            listeners.FastRemove(tableIndex, elementIndex);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        bool FeatureIdContained(int featureId, ulong previousFeatureIds)
        {
            var contained0 = (((int)previousFeatureIds ^ featureId) & 0xFFFF) == 0;
            var contained1 = (((int)(previousFeatureIds >> 16) ^ featureId) & 0xFFFF) == 0;
            var contained2 = (((int)(previousFeatureIds >> 32) ^ featureId) & 0xFFFF) == 0;
            var contained3 = (((int)(previousFeatureIds >> 48) ^ featureId) & 0xFFFF) == 0;
            return contained0 | contained1 | contained2 | contained3;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void UpdatePreviousCollision<TManifold>(ref PreviousCollisionData collision, ref TManifold manifold) where TManifold : struct, IContactManifold<TManifold>
        {
            Debug.Assert(manifold.Count <= 4, "This demo was built on the assumption that nonconvex manifolds will have a maximum of four contacts, but that might have changed.");
            // 如果由于非凸流形容量的更改而命中上述断言,则需要更新它使用的压缩特征id表示。
            // 我非常怀疑非凸集合管是否会使用8个以上的触点,所以解决这个问题不需要太多的改变。
            for (int j = 0; j < manifold.Count; ++j)
            {
                Unsafe.Add(ref collision.FeatureId0, j) = manifold.GetFeatureId(j);
            }
            collision.ContactCount = manifold.Count;
            collision.Fresh = true;
        }

        void HandleManifoldForCollidable<TManifold>(int workerIndex, CollidableReference source, CollidableReference other, CollidablePair pair, ref TManifold manifold) where TManifold : struct, IContactManifold<TManifold>
        {
            // "源"是指事件处理程序(可能)附加到的对象,因此我们查找为其注册的侦听器。
            // (这两个订单都调用了此函数,因此我们将捕获其中任何一个的监听器。)
            if (listeners.GetTableIndices(ref source, out var tableIndex, out var listenerIndex))
            {
                // 这个可碰撞的被注册了。对立面是可碰撞的礼物吗？
                ref var previousCollisions = ref listeners.Values[listenerIndex];
                int previousCollisionIndex = -1;
                for (int i = 0; i < previousCollisions.Count; ++i)
                {
                    ref var collision = ref previousCollisions[i];
                    // 由于‘pack’字段同时包含句柄类型(动态、运动学或静电)和打包到单个位字段中的句柄索引,因此相等的值可以保证我们处理的是相同的可碰撞对象。
                    if (collision.Collidable.Packed == other.Packed)
                    {
                        previousCollisionIndex = i;
                        // 此歧管与现有碰撞相关联。
                        for (int contactIndex = 0; contactIndex < manifold.Count; ++contactIndex)
                        {
                            // 我们可以通过查看联系人特征ID来检查每个联系人是否已经出现在前一帧中。有关FeatureIds的更多信息,请参阅‘PreviousCollisionData’。
                            var featureId = manifold.GetFeatureId(contactIndex);
                            var featureIdIsOld = false;
                            for (int previousContactIndex = 0; previousContactIndex < collision.ContactCount; ++previousContactIndex)
                            {
                                if (featureId == Unsafe.Add(ref collision.FeatureId0, previousContactIndex))
                                {
                                    featureIdIsOld = true;
                                    break;
                                }
                            }
                            if (!featureIdIsOld)
                            {
                                manifold.GetContact(contactIndex, out var offset, out var normal, out var depth, out _);
                                EventHandler.OnContactAdded(source, pair, ref manifold, offset, normal, depth, featureId, contactIndex, workerIndex);
                            }
                        }
                        UpdatePreviousCollision(ref collision, ref manifold);
                        break;
                    }
                }
                if (previousCollisionIndex < 0)
                {
                    // 之前没有发生过碰撞。
                    ref var addsforWorker = ref pendingWorkerAdds[workerIndex];
                    // 如果列表不存在,EnsureCapacity将创建该列表。
                    addsforWorker.EnsureCapacity(Math.Max(addsforWorker.Count + 1, 64), threadDispatcher != null ? threadDispatcher.GetThreadMemoryPool(workerIndex) : pool);
                    ref var pendingAdd = ref addsforWorker.AllocateUnsafely();
                    pendingAdd.ListenerIndex = listenerIndex;
                    pendingAdd.Collision.Collidable = other;
                    UpdatePreviousCollision(ref pendingAdd.Collision, ref manifold);
                    // 为此新歧管中的所有联系人派送事件。
                    for (int i = 0; i < manifold.Count; ++i)
                    {
                        manifold.GetContact(i, out var offset, out var normal, out var depth, out var featureId);
                        EventHandler.OnContactAdded(source, pair, ref manifold, offset, normal, depth, featureId, i, workerIndex);
                    }
                }
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void HandleManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold) where TManifold : struct, IContactManifold<TManifold>
        {
            HandleManifoldForCollidable(workerIndex, pair.A, pair.B, pair, ref manifold);
            HandleManifoldForCollidable(workerIndex, pair.B, pair.A, pair, ref manifold);
        }

        public void Flush()
        {
            // 为简单起见,这完全是连续的。请注意,提取更多并行性在技术上是可能的,但复杂性成本很高,您需要
            // 非常大量的事件正在处理中,以使其物有所值。

            // 删除所有过时的冲突。过时冲突是指本应接收到新的歧管更新但由于歧管不再处于活动状态而没有接收到的冲突。
            for (int i = 0; i < listeners.Count; ++i)
            {
                var collidable = listeners.Keys[i];
                // 涉及不活跃小体的配对不需要检查其新鲜度。如果我们这样做了,就会导致不活动的歧管被认为是拆卸,并且
                // 当身体醒来时,会有更多的接触事件发生。
                if (collidable.Mobility != CollidableMobility.Static && bodies.HandleToLocation[collidable.BodyHandle.Value].SetIndex > 0)
                    continue;
                ref var collisions = ref listeners.Values[i];
                // 注意颠倒顺序。我们在迭代期间移除。
                for (int j = collisions.Count - 1; j >= 0; --j)
                {
                    ref var collision = ref collisions[j];
                    // 再说一次,任何涉及不活跃物体的配对都不需要检查。
                    if (collision.Collidable.Mobility != CollidableMobility.Static && bodies.HandleToLocation[collision.Collidable.BodyHandle.Value].SetIndex > 0)
                        continue;
                    if (!collision.Fresh)
                    {
                        // 尽管此冲突处于活动状态,但自上次刷新以来未更新。它应该被移除。
                        collisions.FastRemoveAt(j);
                        if (collisions.Count == 0)
                        {
                            collisions.Dispose(pool);
                            collisions = default;
                        }
                    }
                    else
                    {
                        collision.Fresh = false;
                    }
                }
            }

            for (int i = 0; i < pendingWorkerAdds.Length; ++i)
            {
                ref var pendingAdds = ref pendingWorkerAdds[i];
                for (int j = 0; j < pendingAdds.Count; ++j)
                {
                    ref var add = ref pendingAdds[j];
                    ref var collisions = ref listeners.Values[add.ListenerIndex];
                    // 确保容量将在必要时初始化插槽。
                    collisions.EnsureCapacity(Math.Max(8, collisions.Count + 1), pool);
                    collisions.AllocateUnsafely() = pendingAdds[j].Collision;
                }
                if (pendingAdds.Span.Allocated)
                    pendingAdds.Dispose(threadDispatcher == null ? pool : threadDispatcher.GetThreadMemoryPool(i));
                // 我们依赖于将延迟初始化的计数清零。
                pendingAdds = default;
            }
        }

        public void Dispose()
        {
            listeners.Dispose(pool);
            for (int i = 0; i < pendingWorkerAdds.Length; ++i)
            {
                Debug.Assert(!pendingWorkerAdds[i].Span.Allocated, "The pending worker adds should have been disposed by the previous flush.");
            }
        }
    }

    public unsafe struct ContactEventCallbacks<TEventHandler> : INarrowPhaseCallbacks where TEventHandler : IContactEventHandler
    {
        ContactEvents<TEventHandler> events;

        public ContactEventCallbacks(ContactEvents<TEventHandler> events)
        {
            this.events = events;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
        {
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
        {
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : struct, IContactManifold<TManifold>
        {
            pairMaterial.FrictionCoefficient = 1f;
            pairMaterial.MaximumRecoveryVelocity = 2f;
            pairMaterial.SpringSettings = new SpringSettings(30, 1);
            events.HandleManifold(workerIndex, pair, ref manifold);
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
        {
            return true;
        }

        public void Initialize(Simulation simulation)
        {
            events.Initialize(simulation.Bodies);
        }

        public void Dispose()
        {
        }

    }

    /// <summary>
    /// 说明如何使用上面的Contact事件处理程序响应新冲突。
    /// </summary>
    public class ContactEventsDemo : Demo
    {
        struct ContactResponseParticle
        {
            public Vector3 Position;
            public float Age;
            public Vector3 Normal;
        }

        struct EventHandler : IContactEventHandler
        {
            public Simulation Simulation;
            public QuickList<ContactResponseParticle> Particles;

            public void OnContactAdded<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold,
                in Vector3 contactOffset, in Vector3 contactNormal, float depth, int featureId, int contactIndex, int workerIndex) where TManifold : struct, IContactManifold<TManifold>
            {
                // VAR OTHER=pair.A.Packed==eventSource.Packed？配对B：配对A;
                // Console.WriteLine($"添加联系人：({eventsource},{其他})：{feature ureId}");
                // 只需忽略分配的空间之外的任何粒子。
                var index = Interlocked.Increment(ref Particles.Count) - 1;
                if (index < Particles.Span.Length)
                {
                    ref var particle = ref Particles[index];

                    // 触点数据根据配对的顺序进行校准,因此使用A的位置很重要。
                    particle.Position = contactOffset + (pair.A.Mobility == CollidableMobility.Static ?
                        new StaticReference(pair.A.StaticHandle, Simulation.Statics).Pose.Position :
                        new BodyReference(pair.A.BodyHandle, Simulation.Bodies).Pose.Position);
                    particle.Age = 0;
                    particle.Normal = contactNormal;
                }
            }
        }

        ContactEvents<EventHandler> events;

        public override void Initialize(ContentArchive content, Camera camera)
        {
            camera.Position = new Vector3(0, 8, -20);
            camera.Yaw = MathHelper.Pi;

            events = new ContactEvents<EventHandler>(new EventHandler(), BufferPool, ThreadDispatcher);
            // PositionFirstTimestpper是最简单的时步模式,但由于它在帧开始时将速度积分到位置中,因此直接修改了时间步长之外的速度
            // 将在碰撞检测或解算器有机会介入之前集成。这在这个演示中很好。其他内置选项包括PositionLastTimestpper和SubsteppingTimestpper。
            // 请注意,timeSteper还具有回调,您可以使用这些回调在处理阶段之间执行逻辑,如BeforeCollisionDetection。
            Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks<EventHandler>(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
            events.EventHandler.Particles = new QuickList<ContactResponseParticle>(128, BufferPool);
            events.EventHandler.Simulation = Simulation;

            var listenedBody1 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 5, 0), 1, Simulation.Shapes, new Box(1, 2, 3)));
            events.RegisterListener(new CollidableReference(CollidableMobility.Dynamic, listenedBody1));

            var listenedBody2 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0.5f, 10, 0), 1, Simulation.Shapes, new Capsule(0.25f, 0.7f)));
            events.RegisterListener(new CollidableReference(CollidableMobility.Dynamic, listenedBody2));

            Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 1, 30)), 0.04f)));
            Simulation.Statics.Add(new StaticDescription(new Vector3(0, 3, 15), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 5, 1)), 0.04f)));

        }

        public override void Update(Window window, Camera camera, Input input, float dt)
        {
            base.Update(window, camera, input, dt);
            events.Flush();

            ref var particles = ref events.EventHandler.Particles;
            // 该计数跨多个线程递增;它可能超出了缓冲区大小。别管额外的了。
            if (particles.Count > particles.Span.Length)
                particles.Count = particles.Span.Length;
            for (int i = particles.Count - 1; i >= 0; --i)
            {
                ref var particle = ref particles[i];
                particle.Age += dt;
                if (particle.Age > 0.7325f)
                {
                    particles.FastRemoveAt(i);
                }
                else
                {
                    particle.Position += particle.Normal * (2 * dt);
                }

            }
        }

        public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
        {
            ref var particles = ref events.EventHandler.Particles;
            for (int i = particles.Count - 1; i >= 0; --i)
            {
                ref var particle = ref particles[i];
                var radius = particle.Age * (particle.Age * (0.135f - 2.7f * particle.Age) + 1.35f);
                var pose = new RigidPose(particle.Position);
                renderer.Shapes.AddShape(new Sphere(radius), Simulation.Shapes, ref pose, new Vector3(0, 1, 0));
            }
            base.Render(renderer, camera, input, text, font);
        }
    }
}
